/***************************************************************************
 创建者: 华磊
 开始时间: 2019.7.4
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写
 ***************************************************************************/
#include "reflectormatch.h"
#include "domparser.h"
#include <QDebug>
#include "frames.hpp"
#include "messagelog.h"
#include "circulecrosscalculate.h"
#include "filteringalgorithm.h"
#include "kalmanfiltersimple.h"

#define D_ALLOW_CLOUD_DISTANCE 0.05
#define D_COLLUM_OFFSET_RATIO 0.45
#define D_REFLECT_POSITION_INITIAL_COUNT 20;



ReflectorMatch::ReflectorMatch(QString configFileIn, int robotIdIn, int laserSkipCountIn)
{

    robotId=robotIdIn;
    laserSkipCount=laserSkipCountIn;
    configFile=configFileIn;
    minConsumed=100000000000;
    currentConsumed=0;
    maxConsumed=0;

    agvInMapPosition.isUpdateSucess=false;
    agvInMapPosition.matchScore=-1;
    agvInMapPosition.maxMatchError=-1;
//    agvInWorldPosition.matchSucessTimeStmap=0;
    agvInMapPosition.position.x=0;
    agvInMapPosition.position.y=0;
    agvInMapPosition.position.rotate=0;

    initialPositionCount=D_REFLECT_POSITION_INITIAL_COUNT;

    currentLaserFilterType=0;
    isMatchSucess=false;

    debugFlag=0;
    isInitialOkStatus=false;
    detectPara.odomOffsetFilterCount=20;

    amclPosition.isUpdateSucess=false;

    frame_baselink_in_laserlink=new KDL::Frame;
    frame_laserlink_in_baselink=new KDL::Frame;
//    agvInMapFrame=new KDL::Frame;
    frame_baselink_in_odom=new KDL::Frame;
    frame_odom_in_map=new KDL::Frame;

    filtering_position_x=new FilteringAlgorithm;
    filtering_position_y=new FilteringAlgorithm;
    filtering_position_r=new FilteringAlgorithm;
    kalmanFilter=new KalmanFilterSimple(0.001,0.05);
    laserDateReadyCount=false;


    QString readComment;
    int tmpReturn = readConfigFile(configFileIn,readComment);
    if(1!=tmpReturn)
    {
        addMsg(ENUM_MSG_ERROR, "ReflectorMatch", "ReflectorMatch", 22200, robotId,0,0,0,0,readComment);
    }
    else
    {
        isInitialOkStatus=true;
    }
    poseErrorInfo.x=detectPara.initial_AgvPositionOffset;
    poseErrorInfo.y=detectPara.initial_AgvPositionOffset;
    poseErrorInfo.rotate=detectPara.initial_AgvAngleOffset;//0.038rad=2.17degree
    matchDebugInfo.odomAccumulatedError.x=0;
    matchDebugInfo.odomAccumulatedError.y=0;
    matchDebugInfo.odomAccumulatedError.rotate=0;

}

int ReflectorMatch::getReflectLaserFilterParameter(int index, LaserFilterParameter &paraOut)
{
    if(index<0)
    {
        paraOut=currentLaserFilter;
        return 1;
    }
    usingMutex.lock();

    if(laserFilter_Parameter.contains(index))
    {
        paraOut=laserFilter_Parameter.value(index);
        usingMutex.unlock();
        return 1;
    }
    else
    {
        qDebug()<<"failed ,,,getReflectLaserFilterParameter index"<<index;
        usingMutex.unlock();
        return 0;
    }
    return -1;
}

int ReflectorMatch::setReflectLaserFilterParameter(int index, LaserFilterParameter paraIn)
{
    if(index<0)
    {
        currentLaserFilter=paraIn;
        return 1;
    }
    usingMutex.lock();
    if(laserFilter_Parameter.contains(index))
    {
        laserFilter_Parameter.remove(index);
        laserFilter_Parameter.insert(index,paraIn);
        if(index==currentLaserFilterType)
        {
            currentLaserFilter=paraIn;
        }
        int tmpKey=writeLaserFilterConfigFile(configFile,laserFilter_Parameter);
        usingMutex.unlock();
        return tmpKey;
    }
    else
    {
        qDebug()<<"failed ,,,getReflectLaserFilterParameter index"<<index;
        usingMutex.unlock();
        return 0;
    }
    return -1;
}

int ReflectorMatch::getReflectCalculateCommonParameter(ReflectCommonDetectPara &paraOut)
{
        paraOut=detectPara;
        return 1;
}

int ReflectorMatch::setReflectCalculateCommonParameter(ReflectCommonDetectPara paraIn)
{
    detectPara=paraIn;
    if(detectPara.correctFilterCount<1)
    {
        detectPara.correctFilterCount=1;
    }
    return writeCommonCalculateConfigFile(configFile,paraIn);
    return 1;
}

int ReflectorMatch::notifyAmclPositionInfo(PositionInfo vehiclePositionIn)
{
    usingMutex.lock();
    amclPosition=vehiclePositionIn;
    usingMutex.unlock();
    return 1;
}



int ReflectorMatch::getAllReferenceMarks(std::vector<ReflectMarkInfo> &reflectMarksOut)
{
    reflectMarksOut.clear();
    usingMutex.lock();
    ReflectMarkInfo tmpMarkInfo;
    foreach (tmpMarkInfo, reflectMarks) {
        reflectMarksOut.push_back(tmpMarkInfo);
    }
    usingMutex.unlock();
    return 1;
}

int ReflectorMatch::addReferenceMarkToMap(ReflectMarkInfo markIn)
{
    usingMutex.lock();


    ReflectMarkInfo tmpMarkInfo;
    foreach(tmpMarkInfo,reflectMarks)
    {
        if(tmpMarkInfo.isPlaneMark!=markIn.isPlaneMark)
        {
            addMsg(ENUM_MSG_ERROR, "ReflectorMatch", "ReflectorMatch", 22203, robotId );
            usingMutex.unlock();
            return -1;
        }
        if(tmpMarkInfo.typeId!=markIn.typeId)
        {
            addMsg(ENUM_MSG_ERROR, "ReflectorMatch", "ReflectorMatch", 22204, robotId ,markIn.typeId);
            usingMutex.unlock();
            return -2;
        }

    }

    if(true ==markIn.isPlaneMark )
    {
        if(false==planeMarkWidth.contains(markIn.typeId))
        {
            addMsg(ENUM_MSG_ERROR, "ReflectorMatch", "ReflectorMatch", 22202, robotId );
            usingMutex.unlock();
            return -3;
        }
        else
        {
            currentPlaneMarkWidth=planeMarkWidth.value(markIn.typeId);
        }
    }
    else
    {
        if(false==columnMarkDiameter.contains(markIn.typeId))
        {
            addMsg(ENUM_MSG_ERROR, "ReflectorMatch", "ReflectorMatch", 22201, robotId );
            usingMutex.unlock();
            return -4;
        }
        else
        {
            currentColumnMarkDiameter=columnMarkDiameter.value(markIn.typeId);
        }
    }

    reflectMarks.insert(markIn.uniqId,markIn);

    usingMutex.unlock();
    qDebug()<<"sucess ,addReferenceMarkToMap ,uniqId"<<markIn.uniqId;
    return 1;
}

int ReflectorMatch::saveReferenceMarkToFile(QString filePathIn)
{
    qDebug() << "saveReferenceMarkToFile begin,,,";
    QMap<int,ReflectMarkInfo> tmpReflectMarks;
    usingMutex.lock();
    tmpReflectMarks= reflectMarks;
    usingMutex.unlock();

    initial_MapFile(filePathIn);

    DomParser domParser;

    bool ok = domParser.openXml(filePathIn, QIODevice::ReadWrite );
    if( !ok )//无法打开机器人数据文件！
    {
        qDebug()<<"error file not exist,saveReferenceMarkToFile！";
        return -1;
    }


    ReflectMarkInfo tmpMarkInfo;
    foreach(tmpMarkInfo,reflectMarks)
    {
        saveOneMarkToXmlFile(&domParser,tmpMarkInfo);
    }

    domParser.closeXml(true);//保存


    return 1;
}


int ReflectorMatch::initial_MapFile(QString filePathIn)
{
    QFile file(filePathIn);
    //定义文件内容字符串
    QString content= "<?xml version='1.0' encoding='UTF-8'?> <Config> <ReflectMarks>  </ReflectMarks>  </Config>";

    //存在打开，不存在创建
    file.open(QIODevice::WriteOnly | QIODevice::Text);
    //写入内容,这里需要转码，否则报错。
    QByteArray str = content.toUtf8();
    //写入QByteArray格式字符串
    file.write(str);

    //关闭文件
    file.close();
    return 1;

}

bool ReflectorMatch::isPlaneLaserMark(const LaserDataInfo &laserDataIn,int tmpValidCount,int lastInValidIndex,LaserMarkInfo &markPointOut)
{
    //判断尺寸是否符合要求
    if(detectPara.matchPlaneMarkMinWidth>0 && tmpValidCount<2)
    {
        return false;
    }
    //开始点
    VehiclePosition tmpStartPoint;
    double startRange=laserDataIn.ranges[lastInValidIndex-tmpValidCount];
    double startAngle=laserDataIn.angle_min+laserDataIn.angle_increment*(lastInValidIndex-tmpValidCount);
    tmpStartPoint.x=startRange*cos(startAngle);
    tmpStartPoint.y=startRange*sin(startAngle);
    //结束点
    VehiclePosition tmpEndPoint;
    double endRange=laserDataIn.ranges[lastInValidIndex-1];
    double endAngle=laserDataIn.angle_min+laserDataIn.angle_increment*(lastInValidIndex-1);
    tmpEndPoint.x=endRange*cos(endAngle);
    tmpEndPoint.y=endRange*sin(endAngle);

    double tmpWidth=sqrt((tmpStartPoint.x-tmpEndPoint.x)*(tmpStartPoint.x-tmpEndPoint.x)
                         +(tmpStartPoint.y-tmpEndPoint.y)*(tmpStartPoint.y-tmpEndPoint.y));

    if(tmpWidth<detectPara.matchPlaneMarkMinWidth || tmpWidth>detectPara.matchPlaneMarkMaxWidth)
    {
        return false;
    }

    double sumRange=0;
    double sumAngle=0;

    for(int k=0;k<tmpValidCount;k++)
    {
        sumRange+=laserDataIn.ranges[lastInValidIndex-tmpValidCount+k];
        sumAngle+=laserDataIn.angle_min+laserDataIn.angle_increment*(lastInValidIndex-tmpValidCount+k);
    }
    sumRange=sumRange/tmpValidCount;
    sumAngle=sumAngle/tmpValidCount;
    //转换为笛卡尔点
    markPointOut.pose_x=sumRange*cos(sumAngle);
    markPointOut.pose_y=sumRange*sin(sumAngle);
    markPointOut.laserRangeValue=sumRange;
    markPointOut.laserAngleValue=sumAngle;
    return true;

}

bool ReflectorMatch::isColumnLaserMark(const LaserDataInfo &laserDataIn, int tmpValidCount,
                                       int lastInValidIndex, LaserMarkInfo &markPointOut)
{
    //判断尺寸是否符合要求
    if(detectPara.matchColumnMarkDiameterTolorenceRatio>0 && tmpValidCount<2)
    {
        return false;
    }
    //开始点
    VehiclePosition tmpStartPoint;
    double startRange=laserDataIn.ranges[lastInValidIndex-tmpValidCount];
    double startAngle=laserDataIn.angle_min+laserDataIn.angle_increment*(lastInValidIndex-tmpValidCount);
    tmpStartPoint.x=startRange*cos(startAngle);
    tmpStartPoint.y=startRange*sin(startAngle);

    //检查有效点数量是否合格
    bool isCheckOk=true;
    for(int i=0;i<validPointCheckList.size();i++)
    {
        if(startRange<validPointCheckList[i].distance)
        {
            if(tmpValidCount-1<validPointCheckList[i].minCount)
            {
                isCheckOk=false;
                if(1==debugFlag)
                {
                     qDebug()<<"微段计算:点数不合格 (tmpValidCount-1)="<<tmpValidCount-1<<"minCount"<<validPointCheckList[i].minCount
                               <<"distance"<<validPointCheckList[i].distance;
                }
            }
            break;
        }
    }

    if(false==isCheckOk)
    {

        return false;
    }

    //结束点
    VehiclePosition tmpEndPoint;
    double endRange=laserDataIn.ranges[lastInValidIndex-1];
    double endAngle=laserDataIn.angle_min+laserDataIn.angle_increment*(lastInValidIndex-1);
    tmpEndPoint.x=endRange*cos(endAngle);
    tmpEndPoint.y=endRange*sin(endAngle);

    double tmpWidth=sqrt((tmpStartPoint.x-tmpEndPoint.x)*(tmpStartPoint.x-tmpEndPoint.x)
                         +(tmpStartPoint.y-tmpEndPoint.y)*(tmpStartPoint.y-tmpEndPoint.y));

    if(tmpWidth<currentColumnMarkDiameter*(1-detectPara.matchColumnMarkDiameterTolorenceRatio)
            || tmpWidth>currentColumnMarkDiameter*(1+detectPara.matchColumnMarkDiameterTolorenceRatio))
    {
        if(1==debugFlag)
        {
             qDebug()<<"微段计算:尺寸不合格tmpWidth"<<tmpWidth;
        }
        return false;
    }

    //根据亮度来设定权重，提高精度．
    double sumRange=0;
    double sumAngle=0;
    double tmpTotalWeight=0;
    for(int k=0;k<tmpValidCount;k++)
    {
        tmpTotalWeight+=laserDataIn.intensities[lastInValidIndex-tmpValidCount+k];
    }

    for(int k=0;k<tmpValidCount;k++)
    {

        double tmpRatio=laserDataIn.intensities[lastInValidIndex-tmpValidCount+k]/tmpTotalWeight;
        sumRange+=laserDataIn.ranges[lastInValidIndex-tmpValidCount+k]*tmpRatio;
        sumAngle+=(laserDataIn.angle_min+laserDataIn.angle_increment*(lastInValidIndex-tmpValidCount+k))*tmpRatio;
//        if(1==debugFlag)
//        {
//             qDebug()<<"intensities tmpRatio"<<tmpRatio;
//        }
    }
//    sumRange=sumRange/tmpValidCount+D_COLLUM_OFFSET_RATIO*currentColumnMarkDiameter;
//    sumAngle=sumAngle/tmpValidCount;
    sumRange=sumRange+D_COLLUM_OFFSET_RATIO*currentColumnMarkDiameter;


    //转换为笛卡尔点
    markPointOut.pose_x=sumRange*cos(sumAngle);
    markPointOut.pose_y=sumRange*sin(sumAngle);
    markPointOut.laserRangeValue=sumRange;
    markPointOut.laserAngleValue=sumAngle;
    if(1==debugFlag)
    {
         qDebug()<<"laserRangeValue"<<sumRange<<"laserAngleValue"<<sumAngle*180.0*M_PI
                 <<"markPointOut.pose_x"<<markPointOut.pose_x<<"pose_y"<<markPointOut.pose_y;
    }
    return true;
}

//#define D_VMARK_POSE_FILTER_COUNT 6
//#define D_VMARK_POSE_FILTER_R_COUNT 8
//int ReflectorMatch::filterPositionResult(double &pose_x,double &pose_y,double &pose_r)
//{
//        pose_x=filtering_position_x->medianMean(
//                                                    pose_x,D_VMARK_POSE_FILTER_COUNT);
//        pose_y=filtering_position_y->medianMean(
//                                                    pose_y,D_VMARK_POSE_FILTER_COUNT);
//        pose_r=filtering_position_r->medianMean_angle(
//                                                    pose_r,D_VMARK_POSE_FILTER_R_COUNT);
//        return 1;

//}

int ReflectorMatch::calculateLaserLinkPosition(VehiclePosition agvInMapPositionIn, VehiclePosition &laserPositionOut)
{
    //agvInMapFrame
    KDL::Frame agvInMapFrame;
    agvInMapFrame.p.data[0]=agvInMapPositionIn.x;
    agvInMapFrame.p.data[1]=agvInMapPositionIn.y;
    agvInMapFrame.p.data[2]=0;
    agvInMapFrame.M=agvInMapFrame.M.RPY(0,0,agvInMapPositionIn.rotate);

    //frame_agv_in_map*frame_laserlink_in_agv=frame_laserlink_in_map
    KDL::Frame tmpframe_laserlink_in_map=agvInMapFrame*(*frame_laserlink_in_baselink);

    double tmp_roll,tmp_pitch,tmp_yaw;
    laserPositionOut.x=tmpframe_laserlink_in_map.p[0];
    laserPositionOut.y=tmpframe_laserlink_in_map.p[1];
    tmpframe_laserlink_in_map.M.GetRPY(tmp_roll,tmp_pitch,tmp_yaw);
    laserPositionOut.rotate=tmp_yaw;
    return 1;
}



int ReflectorMatch::saveOneMarkToXmlFile(DomParser *domParser,ReflectMarkInfo markInfo)
{
    ///读内部轴参数
    bool ok;
    QDomNode  mainNode = domParser->findSubNode(domParser->getRootDomElement(), "ReflectMarks", ok );
    if( !ok )
    {
       qDebug()<<"error no ReflectMarks node";
        return -1;
    }


    domParser->addNode(mainNode,"element","");
    QDomNode  recordNode;
    QDomNodeList devNodes = mainNode.childNodes();
    recordNode=devNodes.at(devNodes.size()-1);
    domParser->addNode(recordNode,"uniqId",markInfo.uniqId);
    domParser->addNode(recordNode,"isPlaneMark",markInfo.isPlaneMark);
    domParser->addNode(recordNode,"typeId",markInfo.typeId);
    domParser->addNode(recordNode,"groupId",markInfo.groupId);
    domParser->addNode(recordNode,"pose_x",markInfo.pose_x);
    domParser->addNode(recordNode,"pose_y",markInfo.pose_y);
    domParser->addNode(recordNode,"pose_r",markInfo.pose_r);


    return 1;

}


int ReflectorMatch::deleteReferenceMarkInMap(int uniqId)
{
    usingMutex.lock();
    if(reflectMarks.contains(uniqId))
    {
            reflectMarks.remove(uniqId);
            qDebug()<<"sucess ,deleteReferenceMarkInMap ,uniqId"<<uniqId;
    }
    else
    {
        qDebug()<<"failed ,deleteReferenceMarkInMap ,no uniqId"<<uniqId;
    }

    usingMutex.unlock();

    return 1;
}

int ReflectorMatch::getReflectPositionCertainity(VehiclePosition &uncertainPoseOut)
{
    uncertainPoseOut=matchDebugInfo.odomAccumulatedError;//poseErrorInfo
    return 1;
}



int ReflectorMatch::setLaserIntensityFilter(double minumIntensityIn, double maxLaserRangeIn,
                                            double minAngleIn, double maxAngleIn)
{
    currentLaserFilter.minumIntensity=minumIntensityIn;
    currentLaserFilter.maxLaserRange=maxLaserRangeIn;
    currentLaserFilter.minAngle=minAngleIn;
    currentLaserFilter.maxAngle=maxAngleIn;

    return 1;

}

int ReflectorMatch::getDebugInfo(ReflectDebugInfo &matchDebugInfoOut)
{
    usingMutex.lock();

    matchDebugInfoOut=matchDebugInfo;
    matchDebugInfoOut.minumIntensity=currentLaserFilter.minumIntensity;
    matchDebugInfoOut.minLaserRange=currentLaserFilter.minLaserRange;
    matchDebugInfoOut.maxLaserRange=currentLaserFilter.maxLaserRange;
    matchDebugInfoOut.pose_x=agvInMapPosition.position.x;
    matchDebugInfoOut.pose_y=agvInMapPosition.position.y;
    matchDebugInfoOut.pose_r=agvInMapPosition.position.rotate;
    matchDebugInfoOut.positionError=poseErrorInfo;

    usingMutex.unlock();
    return 1;
}

int ReflectorMatch::setWheelOdometerData_noRos(OdemeterData odemeterDataIn)
{
    usingMutex.lock();

    odemeterData=odemeterDataIn;
    frame_baselink_in_odom->p[0]=odemeterDataIn.pos.x;
    frame_baselink_in_odom->p[1]=odemeterDataIn.pos.y;
    frame_baselink_in_odom->p[2]=0;
    frame_baselink_in_odom->M=frame_baselink_in_odom->M.RPY(0,0,odemeterDataIn.pos.rotate);
    usingMutex.unlock();
    return 1;
}

int ReflectorMatch::laserFilter_test(const LaserDataInfo &frontLaserDataIn, LaserDataInfo &frontLaserDataOut)
{
    return laserFilter_reflect(frontLaserDataIn,frontLaserDataOut);
}


int ReflectorMatch::setInitialVehiclePosition(VehiclePosition agvPoseIn)
{
    usingMutex.lock();

    initialPositionCount=D_REFLECT_POSITION_INITIAL_COUNT;

    matchDebugInfo.odomAccumulatedError.x=0;
    matchDebugInfo.odomAccumulatedError.y=0;
    matchDebugInfo.odomAccumulatedError.rotate=0;

    agvInMapPosition.position=agvPoseIn;
    agvInMapPosition_old=agvInMapPosition;
    poseErrorInfo.x=detectPara.initial_AgvPositionOffset;
    poseErrorInfo.y=detectPara.initial_AgvPositionOffset;
    poseErrorInfo.rotate=detectPara.initial_AgvAngleOffset;//0.038rad=2.17degree
    //求校正矩阵
    calculateOffsetMatrix(agvInMapPosition.position,frame_baselink_in_odom,frame_odom_in_map);

    //求激光雷达位置
    calculateLaserLinkPosition(agvInMapPosition.position,laserlinkInMapPosition);

    usingMutex.unlock();
    return 1;
}

int ReflectorMatch::setDebugFlag(int flagIn)
{
    debugFlag=flagIn;
    return 1;

}

int ReflectorMatch::loadReflectMapFile(QString filePathIn)
{
    QMap<int,ReflectMarkInfo> tmpReflectMarks;

    DomParser xmlParser;

    bool ok = xmlParser.openXml(filePathIn, QIODevice::ReadOnly );
    if( !ok )//无法打开文件！
    {
        addMsg(ENUM_MSG_REMIND, "ReflectorMatch", "ReflectorMatch", 22206, robotId );
       qDebug()<<"error, ReflectorMatch::loadReflectMapFile open file error！";
       usingMutex.lock();
       reflectMarks.clear();
       usingMutex.unlock();
       return -1;
    }

    //PlaneMark
    QDomNode  inNode;
    inNode=xmlParser.findSubNode(xmlParser.getRootDomElement(), "ReflectMarks", ok);
    if( !ok )
    {
        qDebug()<<"error, ReflectMarks node error！";
        return -1;
    }
    QDomNodeList tmpNodelist=inNode.childNodes();

    int tmpTypeId;
    bool tmpIsPlane;

    ReflectMarkInfo tmpInfo;
    for(int i=0;i<tmpNodelist.size();i++)
    {
        //节点ｒｅａｄ数据 typeId
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "isPlaneMark", ok), tmpInfo.isPlaneMark );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }

        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "typeId", ok), tmpInfo.typeId );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        if(0==i)
        {
            tmpTypeId=tmpInfo.typeId;
            tmpIsPlane=tmpInfo.isPlaneMark;
            if(true ==tmpIsPlane )
            {
                if(false==planeMarkWidth.contains(tmpTypeId))
                {
                    addMsg(ENUM_MSG_ERROR, "ReflectorMatch", "ReflectorMatch", 22202, robotId );
                    return -1;
                }
                else
                {
                    currentPlaneMarkWidth=planeMarkWidth.value(tmpTypeId);
                }
            }
            else
            {
                if(false==columnMarkDiameter.contains(tmpTypeId))
                {
                    addMsg(ENUM_MSG_ERROR, "ReflectorMatch", "ReflectorMatch", 22201, robotId );
                    return -2;
                }
                else
                {
                    currentColumnMarkDiameter=columnMarkDiameter.value(tmpTypeId);
                }
            }
        }
        else
        {
            if(tmpTypeId!=tmpInfo.typeId)
            {
                addMsg(ENUM_MSG_ERROR, "ReflectorMatch", "ReflectorMatch", 22204, robotId ,tmpInfo.typeId);
                return -3;
            }

            if(tmpIsPlane!=tmpInfo.isPlaneMark)
            {
                addMsg(ENUM_MSG_ERROR, "ReflectorMatch", "ReflectorMatch", 22203, robotId );
                return -4;
            }
        }

        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "uniqId", ok), tmpInfo.uniqId );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }

        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "groupId", ok), tmpInfo.groupId );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }

        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "pose_x", ok), tmpInfo.pose_x );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }

        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "pose_y", ok), tmpInfo.pose_y );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }

        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "pose_r", ok), tmpInfo.pose_r );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }

        tmpReflectMarks.insert(tmpInfo.uniqId,tmpInfo);
    }



    xmlParser.closeXml( );

    usingMutex.lock();
    reflectMarks=tmpReflectMarks;
    usingMutex.unlock();

    return 1;
}

int ReflectorMatch::setLaserIntensityFilterByType(int typeIn)
{
    filtering_position_x->resetAll();
    filtering_position_y->resetAll();
    filtering_position_r->resetAll();

    usingMutex.lock();
    if(laserFilter_Parameter.contains(typeIn))
    {
        currentLaserFilterType=typeIn;
        currentLaserFilter=laserFilter_Parameter.value(typeIn);
        usingMutex.unlock();
        return 1;
    }
    else
    {
        qDebug()<<"failed ,,,setLaserIntensityFilterByType typeIn"<<typeIn;
        usingMutex.unlock();
        return -1;
    }
    return -1;

}

int ReflectorMatch::getTimeConsumed(double &minConsumedOut, double &currentConsumedOut, double &maxConsumedOut)
{
    minConsumedOut=minConsumed;
    currentConsumedOut=currentConsumed;
    maxConsumedOut=maxConsumed;
    return 1;
}

int ReflectorMatch::resetRosMeasureTime()
{
    minConsumed=100000;
    maxConsumed=-100000;
    return 1;
}

int ReflectorMatch::setLaserSensorData(const LaserDataInfo *laserData)
{
    usingMutex.lock();
    laserData_received=*laserData;
    laserDateReadyCount++;
    usingMutex.unlock();
    return 1;

}

int ReflectorMatch::detectReflectMark_filter(PositionInfo &vehiclePositionInfoOut)
{
    int tmpKey=detectReflectMark(vehiclePositionInfoOut);
    //todo,应该对偏差进行滤波，而不是最终结果
//    filterPositionResult(vehiclePositionInfoOut.position.x,vehiclePositionInfoOut.position.y,
//                         vehiclePositionInfoOut.position.rotate);
    return tmpKey;
}

int ReflectorMatch::detectReflectMark(PositionInfo &vehiclePositionInfoOut)
{

    //位置修正
    if(detectPara.isCorrectByAmcl)
    {
        if(1==calculateCorrectPosition(amclPosition,agvInMapPosition))
        {
            usingMutex.lock();
            //求校正矩阵
            calculateOffsetMatrix(agvInMapPosition.position,frame_baselink_in_odom,frame_odom_in_map);
            //求激光雷达位置
            calculateLaserLinkPosition(agvInMapPosition.position,laserlinkInMapPosition);
            //误差限制
            if(poseErrorInfo.x>detectPara.initial_AgvPositionOffset)
            {
                poseErrorInfo.x=detectPara.initial_AgvPositionOffset;
            }
            if(poseErrorInfo.y>detectPara.initial_AgvPositionOffset)
            {
                poseErrorInfo.y=detectPara.initial_AgvPositionOffset;
            }
            if(poseErrorInfo.rotate>detectPara.initial_AgvAngleOffset)
            {
                poseErrorInfo.rotate=detectPara.initial_AgvAngleOffset;
            }
            usingMutex.unlock();
        }

    }


    //todo  agvInMapPosition.isUpdateSucess=true判断逻辑修改
    if(1==debugFlag)
    {
           qDebug()<<"---------------------ReflectorMatch::detectReflectMark";
    }
    if(laserDateReadyCount<=laserSkipCount)
    {
        calculateAgvPosition_byOdom(agvInMapPosition);
        vehiclePositionInfoOut=agvInMapPosition;
//        qDebug()<<"laserDateReadyCount<=laserSkipCount"<<vehiclePositionInfoOut.position.x
//                  <<vehiclePositionInfoOut.position.y
//                    <<vehiclePositionInfoOut.position.rotate;
        return 1;
    }


    //时间分析
    struct timespec startTime,endTime;
    clock_gettime(CLOCK_MONOTONIC, &startTime);

    usingMutex.lock();
    laserDateReadyCount=false;

    //判断范围
    if(laserData_received.range_max<currentLaserFilter.maxLaserRange
            && laserData_received.range_max>1)//出现过一次探测距离变为０
    {
        currentLaserFilter.maxLaserRange=laserData_received.range_max;
    }
    //过滤激光
    laserFilter_reflect(laserData_received,laserData_received);
    QVector<ReflectMarkInfo> referenceMarks;
    //获取合格的地图标记点（距离车半径范围内）
    getReferenceMarks(referenceMarks);
    matchDebugInfo.initialReferenceMarkCount=referenceMarks.size();
    if(0==referenceMarks.size())
    {
        matchDebugInfo.isMatchSucess=false;
        agvInMapPosition.isUpdateSucess=false;
        calculateAgvPosition_byOdom(agvInMapPosition);
        vehiclePositionInfoOut=agvInMapPosition;
        usingMutex.unlock();
        return 0;
    }

    //agvInMapFrame
    KDL::Frame agvInMapFrame;
    agvInMapFrame.p.data[0]=agvInMapPosition.position.x;
    agvInMapFrame.p.data[1]=agvInMapPosition.position.y;
    agvInMapFrame.p.data[2]=0;
    agvInMapFrame.M=agvInMapFrame.M.RPY(0,0,agvInMapPosition.position.rotate);

    QVector<LaserMarkInfo> laserMarks;
    //获取合格的激光点
    getQualifiedLaserMarkPoints(&agvInMapFrame,laserData_received,referenceMarks[0],laserMarks);
    matchDebugInfo.laserMarks=laserMarks.toStdVector();
    matchDebugInfo.initialLaserMarkCount=laserMarks.size();

    QVector<PointMatchInfo> matchPointOut;
    //标记点和激光点匹配
    calculateMatchPoints(referenceMarks,laserMarks,matchPointOut);

    matchDebugInfo.referenceMarks.clear();
    for(int i=0;i<matchPointOut.size();i++)
    {
//        for(int j=0;j<matchPointOut.size();j++)//去除重复的参考点
//        {

//        }
        matchDebugInfo.referenceMarks.push_back(matchPointOut[i].referencePoint);
    }

    KDL::Frame frame_laser;
    //匹配信息计算出激光雷达位置
    int validCrossPointCountOut;
    if(1==calculateLaserFramePose(matchPointOut,&frame_laser,validCrossPointCountOut))
    {
        //根据雷达位置计算车位置
        if(validCrossPointCountOut>=detectPara.minimumReferencePointCount)
        {
            if(1==calculateAgvPosition_byReflect(&frame_laser,agvInMapPosition.position))
            {
                //复位误差估计
                poseErrorInfo.x=detectPara.mached_AgvPositionOffset;
                poseErrorInfo.y=detectPara.mached_AgvPositionOffset;
                poseErrorInfo.rotate=detectPara.mached_AgvAngleOffset;

                matchDebugInfo.odomAccumulatedError.x=0;
                matchDebugInfo.odomAccumulatedError.y=0;
                matchDebugInfo.odomAccumulatedError.rotate=0;


                agvInMapPosition.matchScore=validCrossPointCountOut;
                agvInMapPosition.isUpdateSucess=true;

                agvInMapPosition.maxMatchError=fabs(poseErrorInfo.x)+fabs(poseErrorInfo.y)+fabs(poseErrorInfo.rotate);
                vehiclePositionInfoOut=agvInMapPosition;

                agvInMapPosition_old=agvInMapPosition;

                //求校正矩阵
                calculateOffsetMatrix(agvInMapPosition.position,frame_baselink_in_odom,frame_odom_in_map);

                //求激光雷达位置
                calculateLaserLinkPosition(agvInMapPosition.position,laserlinkInMapPosition);

                matchDebugInfo.isMatchSucess=true;
                if(1==debugFlag)
                {
                       qDebug()<<"sucess, 基于反光标记新位置　x"<<agvInMapPosition.position.x<<" y"<<agvInMapPosition.position.y
                              <<" r(degree)"<<agvInMapPosition.position.rotate*180/M_PI;
                }
            }
            else
            {
                agvInMapPosition.isUpdateSucess=false;
                agvInMapPosition.matchScore=0;
                calculateAgvPosition_byOdom(agvInMapPosition);
                vehiclePositionInfoOut=agvInMapPosition;
                matchDebugInfo.isMatchSucess=false;
            }


        }
        else
        {
            agvInMapPosition.isUpdateSucess=false;
            agvInMapPosition.matchScore=validCrossPointCountOut;
            calculateAgvPosition_byOdom(agvInMapPosition);
            vehiclePositionInfoOut=agvInMapPosition;
            matchDebugInfo.isMatchSucess=false;
        }
    }
    else
    {
        agvInMapPosition.isUpdateSucess=false;
        agvInMapPosition.matchScore=0;
        calculateAgvPosition_byOdom(agvInMapPosition);
        vehiclePositionInfoOut=agvInMapPosition;
        matchDebugInfo.isMatchSucess=false;
    }


    usingMutex.unlock();


    clock_gettime(CLOCK_MONOTONIC, &endTime);
    currentConsumed=1000000000*(endTime.tv_sec-startTime.tv_sec)+endTime.tv_nsec-startTime.tv_nsec;
    currentConsumed=currentConsumed/1000.0;
    if(currentConsumed<minConsumed)
    {
        minConsumed=currentConsumed;
    }
    if(currentConsumed>maxConsumed)
    {
        maxConsumed=currentConsumed;
    }
    matchDebugInfo.minConsumed=minConsumed;
    matchDebugInfo.currentConsumed=currentConsumed;
    matchDebugInfo.maxConsumed=maxConsumed;

    return 1;
}

int ReflectorMatch::setLaserLinkInBaseLink(double x, double y, double z, double roll, double pitch, double yaw)
{
    usingMutex.lock();
    laser_roll=roll;
    laser_pitch=pitch;
    laser_yaw=yaw;

    KDL::Rotation tmpRotation;
    tmpRotation=tmpRotation.RPY(roll,pitch,yaw);
//    laser_baslink_pitch=pitch;
    KDL::Vector tmpVector(x,y,z);
    *frame_laserlink_in_baselink =KDL::Frame(tmpRotation,tmpVector);
    *frame_baselink_in_laserlink=frame_laserlink_in_baselink->Inverse();
    usingMutex.unlock();
    setLaserIntensityFilterByType(0);//默认使用０号

    return 1;
}

bool comLittle(CrossPointInfo&a, CrossPointInfo&b)
{
    if (a.r1+a.r2 > b.r1+b.r2)
    {
        return false;
    }
    else
    {
        return true;
    }
}

int ReflectorMatch::checkCrossPoints( QVector<CrossPointInfo> &tmpCrossInfoList, int maxAllowMatchNum,
                                      double idealRadius, int idealMatchNum)
{
    //从小到大排序
    qSort(tmpCrossInfoList.begin(), tmpCrossInfoList.end(), comLittle);


    //判断精确范围内是否有足够匹配数。
    QVector<CrossPointInfo> idealCrossInfoList;
    for(int i=0;i<tmpCrossInfoList.size();i++)
    {
        if(tmpCrossInfoList[i].r1<idealRadius && tmpCrossInfoList[i].r2<idealRadius)
        {
            idealCrossInfoList.append(tmpCrossInfoList[i]);
        }
    }
    if(idealCrossInfoList.size()>=idealMatchNum)
    {
        //选择距离最近的匹配点
        tmpCrossInfoList.clear();
        for(int i=0;i<idealMatchNum;i++)
        {
            tmpCrossInfoList.append(idealCrossInfoList[i]);
//            qDebug()<<"半径sum"<<idealCrossInfoList[i].r1+idealCrossInfoList[i].r2<<"i"<<i;
        }
//        qDebug()<<"理想半径内，候选匹配数"<<idealCrossInfoList.size()<<"最终数"<<idealMatchNum;
        return 1;

    }

    //限制总匹配数
    if(tmpCrossInfoList.size()<maxAllowMatchNum)
    {
//        qDebug()<<"匹配数小于最大数";
        return 0;
    }
    while(tmpCrossInfoList.size()>maxAllowMatchNum)
    {
        tmpCrossInfoList.pop_back();
    }
//    qDebug()<<"匹配数大于最大数";
    return 1;


}

bool ReflectorMatch::isCrossAngleOk(double allowMinAngleIn, circle_t circlesIn[], point_t pointsIn, double &angleOut)
{
    double tmpAngle1=atan2(circlesIn[0].center.y-pointsIn.y,circlesIn[0].center.x-pointsIn.x);
    double tmpAngle2=atan2(circlesIn[1].center.y-pointsIn.y,circlesIn[1].center.x-pointsIn.x);
    angleOut=angleRotationJudgement(tmpAngle1,tmpAngle2);
    if(fabs(angleOut)<allowMinAngleIn)
    {
        return false;
    }

    return true;

}


int ReflectorMatch::calculateAgvPositionProbolityError(VehiclePosition oldPoseIn,VehiclePosition newPoseIn,
                                                       VehiclePosition &poseErrorInfoOut)
{
    double delta_x=fabs(oldPoseIn.x-newPoseIn.x);
    double delta_y=fabs(oldPoseIn.y-newPoseIn.y);
    double delta_angle=fabs(angleRotationJudgement(newPoseIn.rotate,oldPoseIn.rotate));
    double length=sqrt(delta_x*delta_x+delta_y*delta_y);
    double angleError=delta_angle*detectPara.odomErrorRatio_angle;
    double lErrorByAngle=length*sin(angleError);

    poseErrorInfoOut.rotate+=angleError;
//    double allowAngleTolorence=30.0;
//    if(fabs(poseErrorInfoOut.rotate)>allowAngleTolorence/180.0*M_PI)
//    {
//        poseErrorInfoOut.rotate-=angleError;
////        addMsg(ENUM_MSG_WARNING, "ReflectorMatch", "ReflectorMatch", 22205, robotId, allowAngleTolorence);
//        return -1;
//    }
    poseErrorInfoOut.x+=delta_x*detectPara.odomErrorRatio_position+lErrorByAngle*cos(M_PI_2-oldPoseIn.rotate);
    poseErrorInfoOut.y+=delta_y*detectPara.odomErrorRatio_position+lErrorByAngle*sin(M_PI_2-oldPoseIn.rotate);

    return 1;
}

int ReflectorMatch::laserFilter_reflect(const LaserDataInfo &laserDataIn, LaserDataInfo &laserDataOut)
{
    laserDataOut=laserDataIn;
    for(int i=0;i<laserDataOut.intensities.size();i++)
    {
        double tmpAngle=laserDataIn.angle_min+i*laserDataIn.angle_increment;
        if(laserDataOut.intensities[i]<currentLaserFilter.minumIntensity-laserDataOut.ranges[i]*detectPara.laserIntensityLoseRatio
                || laserDataOut.ranges[i]>currentLaserFilter.maxLaserRange
                || laserDataOut.ranges[i]<currentLaserFilter.minLaserRange
                || tmpAngle<currentLaserFilter.minAngle || tmpAngle> currentLaserFilter.maxAngle)
        {
            laserDataOut.ranges[i]=laserDataOut.range_max+10000;
        }
    }

    return 1;

}

int ReflectorMatch::getReferenceMarks(QVector<ReflectMarkInfo> &marksOut)
{
    marksOut.clear();
    QVector<double > distanceList;
    double tmpDistance,tmpDistanceX,tmpDistanceY;
    ReflectMarkInfo tmpMark;
    foreach(tmpMark,reflectMarks)
    {
        //reflectMarks被多加了０号ｋｅｙ,不能用reflectMarks[i]
        tmpDistanceX=fabs(tmpMark.pose_x-agvInMapPosition.position.x);
        tmpDistanceY=fabs(tmpMark.pose_y-agvInMapPosition.position.y);
        if(tmpDistanceX<currentLaserFilter.maxLaserRange
                && tmpDistanceY<currentLaserFilter.maxLaserRange)
        {
            marksOut.append(tmpMark);
            tmpDistance=(tmpDistanceX+tmpDistanceY)/2.0;
            distanceList.append(tmpDistance);
        }
    }
    //由近到远排序
//    double middleValue;
//    ReflectMarkInfo middleMark;
//    for(int i=0;i<distanceList.size()-1;i++)
//    {
//        for(int j=i+1;j<distanceList.size();j++)
//        {
//            if(distanceList[i]>distanceList[j])
//            {
//                middleValue=distanceList[i];
//                distanceList[i]=distanceList[j];
//                distanceList[j]=distanceList[i];

//                middleMark=marksOut[i];
//                marksOut[i]=marksOut[j];
//                marksOut[j]=middleMark;
//            }
//        }
//    }

    return 1;
}

int ReflectorMatch::getQualifiedLaserMarkPoints(KDL::Frame *agvInMapFrame,const LaserDataInfo &laserDataIn,
                                    ReflectMarkInfo markInfo,QVector<LaserMarkInfo> &laserMarkOut)
{
    laserMarkOut.clear();
    QVector<LaserMarkInfo> tmpPoints;
    LaserMarkInfo tmpMarkPoint;
    //是否需要判断１Ｍark上面反射的有效点有几个？

    //是否要求平均值，作为mark中心?

    //先把连续点合并为１个点，判断距离也要连续
    int tmpValidCount=0;//微段有效点云个数
    double lastValidRange=0;
    bool isReadyMacroSegment=false;
    int validLastPointIndex;
    for(int i=0;i<laserDataIn.intensities.size();i++)
    {
        if(laserDataIn.ranges[i]<laserDataIn.range_max)
        {
            tmpValidCount++;
            if(1==tmpValidCount)
            {
                lastValidRange=laserDataIn.ranges[i];
            }
            else if(1<tmpValidCount)
            {
                lastValidRange=laserDataIn.ranges[i-1];
            }

            //距离连续判断．反光柱边上可能有一个飞点．
            if(3<=tmpValidCount && fabs(laserDataIn.ranges[i]-lastValidRange)>D_ALLOW_CLOUD_DISTANCE)
            {
                validLastPointIndex=i-2;
                tmpValidCount=tmpValidCount-3;
                isReadyMacroSegment=true;

            }
            else if(2==tmpValidCount && fabs(laserDataIn.ranges[i]-lastValidRange)>D_ALLOW_CLOUD_DISTANCE)
            {
                tmpValidCount=0;
            }

        }

        if(2<=tmpValidCount &&laserDataIn.ranges[i]>laserDataIn.range_max )
        {
            validLastPointIndex=i-2;
            tmpValidCount=tmpValidCount-2;
            isReadyMacroSegment=true;
        }

        //微段计算
        if(isReadyMacroSegment)
        {
            if(1==debugFlag)
            {
                 qDebug()<<"微段计算tmpValidCount"<<tmpValidCount
                            <<"validLastPointIndex"<<validLastPointIndex;
            }

            if(markInfo.isPlaneMark)
            {
                if(isPlaneLaserMark(laserDataIn, tmpValidCount, validLastPointIndex,tmpMarkPoint))
                {
                    tmpPoints.append(tmpMarkPoint);
                }


            }
            else
            {
                if(isColumnLaserMark(laserDataIn, tmpValidCount, validLastPointIndex,tmpMarkPoint))
                {
                    tmpPoints.append(tmpMarkPoint);
                }

            }
            isReadyMacroSegment=false;
            tmpValidCount=0;//重新计算微段
        }
    }

    //剔除间隔距离小于设定值的点
    QVector<int> tmpInvalidPointIndex;
    for(int i=0;i<tmpPoints.size()-1;i++)
    {
        for(int j=i+1;j<tmpPoints.size();j++)
        {
            if(fabs(tmpPoints[i].pose_x-tmpPoints[j].pose_x)<detectPara.allowMinMarkDistance &&
                    fabs(tmpPoints[i].pose_y-tmpPoints[j].pose_y)<detectPara.allowMinMarkDistance )
            {
                tmpInvalidPointIndex.append(i);
                tmpInvalidPointIndex.append(j);
            }
        }
    }




    bool isValid=true;
    for(int i=0;i<tmpPoints.size();i++)
    {
        isValid=true;
        for(int j=0;j<tmpInvalidPointIndex.size();j++)
        {
            if(i== tmpInvalidPointIndex[j])
            {
                if(1==debugFlag)
                {
                    qDebug()<<"剔除间隔距离小于设定值的laserPoint点"<<tmpInvalidPointIndex[j];
                }

                isValid=false;

            }

        }

        if(isValid)
        {
            //转移到地图坐标系
            trasferToMapCoordinate(agvInMapFrame,tmpPoints[i]);
            laserMarkOut.append(tmpPoints[i]);
        }
    }

    //由近到远排序
//    VehiclePosition middleMark;
//    for(int i=0;i<laserMarkOut.size()-1;i++)
//    {
//        for(int j=i+1;j<laserMarkOut.size();j++)
//        {
//            if(laserMarkOut[i].rotate>laserMarkOut[j].rotate)
//            {
//                middleMark=laserMarkOut[i];
//                laserMarkOut[i]=laserMarkOut[j];
//                laserMarkOut[j]=middleMark;
//            }
//        }
//    }
    return 1;

}

struct KeyError
{
    int key;
    double error;
};

int ReflectorMatch::calculateMatchPoints(const QVector<ReflectMarkInfo> &referenceMarks,
                                         const QVector<LaserMarkInfo> &laserPoints,
                                         QVector<PointMatchInfo> &matchPointOut)
{
    if(1==debugFlag)
    {
        qDebug()<<"初始referenceMarks"<<referenceMarks.size()<<"初始laserPoints"<<laserPoints.size();
    }
    matchPointOut.clear();

    double le;
    double sinA=sin(poseErrorInfo.rotate);
    double xError,yError;
    QVector<KeyError> tmpErrorList;
    KeyError tmpError;
    PointMatchInfo tmpMatchInfo;
    for(int i=0;i<laserPoints.size();i++)
    {
        tmpErrorList.clear();
        le=laserPoints[i].laserRangeValue*sinA;
        for(int j=0;j<referenceMarks.size();j++)
        {
            //车的位置误差估计，去选取激光点。//忽略车角度估计误差引起的ｘy方向误差不一样大，做了简化
            xError=fabs(laserPoints[i].pose_x-referenceMarks[j].pose_x);
            yError=fabs(laserPoints[i].pose_y-referenceMarks[j].pose_y);
            if(xError<poseErrorInfo.x+le && yError <poseErrorInfo.y+le)
            {
                tmpError.key=j;
                tmpError.error=xError+yError;
                tmpErrorList.append(tmpError);
            }
        }
        //挑选误差最小的
        if(0<tmpErrorList.size())
        {
            int minIndex=0;
            double minValue=tmpErrorList[0].error;
            for(int k=1;k<tmpErrorList.size();k++)
            {
                if(tmpErrorList[k].error<minValue)
                {
                    minIndex=k;
                    minValue=tmpErrorList[k].error;
                }


            }

            if(1==debugFlag)
            {
                qDebug()<<"1激光点对应参考点个数"<<tmpErrorList.size()<<"i"<<i;
            }

            tmpMatchInfo.laserPoint=laserPoints[i];
            tmpMatchInfo.matchDistanceError=tmpErrorList[minIndex].error;
            tmpMatchInfo.referencePoint=referenceMarks[tmpErrorList[minIndex].key];
            matchPointOut.append(tmpMatchInfo);

        }

    }

    //可能出现1个参考点匹配多个激光点的问题,需要过滤
    QVector<PointMatchInfo> tmpMatchFilter;
    QVector<int> tmpIdList;
//    if(debugFlag)
//    {
//        qDebug()<<"1 matchPointOut.size"<<matchPointOut.size();
//    }

    if(2<=matchPointOut.size())
    {
        tmpIdList.append(matchPointOut[0].referencePoint.uniqId);
        tmpMatchFilter.append(matchPointOut[0]);

        for(int m=0;m<matchPointOut.size();m++)
        {
//            if(debugFlag)
//            {
//                qDebug()<<"m"<<m<<"uniqId"<<matchPointOut[m].referencePoint.uniqId;
//            }

            double tmpMatchMinError=matchPointOut[m].matchDistanceError;
            int tmpMatchMinIndex=m;
            for(int n=1;n<matchPointOut.size();n++)
            {

                if(matchPointOut[m].referencePoint.uniqId==matchPointOut[n].referencePoint.uniqId)
                {
                    if(matchPointOut[n].matchDistanceError<matchPointOut[m].matchDistanceError)
                    {
                        tmpMatchMinError=matchPointOut[n].matchDistanceError;
                        tmpMatchMinIndex=n;
//                        if(debugFlag)
//                        {
//                            qDebug()<<"参考点重复匹配激光点 error1"<<matchPointOut[n].matchDistanceError
//                                   <<"error2"<<matchPointOut[m].matchDistanceError;
//                        }
                    }
                }

            }
            bool isContained=false;
            for(int i=0;i<tmpIdList.size();i++)
            {
                if(tmpIdList[i]==matchPointOut[tmpMatchMinIndex].referencePoint.uniqId)
                {
                    isContained=true;
                }
            }
            if(false==isContained)
            {
                tmpMatchFilter.append(matchPointOut[tmpMatchMinIndex]);
                tmpIdList.append(matchPointOut[tmpMatchMinIndex].referencePoint.uniqId);
//                if(debugFlag)
//                {
//                    qDebug()<<"tmpIdList.append"<<matchPointOut[tmpMatchMinIndex].referencePoint.uniqId;
//                }
            }

        }

        matchPointOut=tmpMatchFilter;
    }
    else
    {

    }

    if(debugFlag)
    {
        qDebug()<<"tmpIdList"<<tmpIdList<<"final matchPointOut.size()"<<matchPointOut.size();
    }
    return 1;

}

int ReflectorMatch::trasferToMapCoordinate(KDL::Frame *agvInMapFrame,LaserMarkInfo &pointOut)
{
    //laserPoint 在　laserlink 矩阵表示
    KDL::Rotation tmpRotation;
    tmpRotation=tmpRotation.RPY(0,0,pointOut.pose_r);
//    KDL::Vector tmpVector2(localCompare_originY*mPerPix,localCompare_originX*mPerPix,0);
    KDL::Vector tmpVector(pointOut.pose_x,pointOut.pose_y,0);
    KDL::Frame frame_laserPoint_in_laserlink(tmpRotation,tmpVector);
//    qDebug()<<"frame_laser_in_image"<<"x="<<frame_laser_in_image.p[0]<<" ,y="<<frame_laser_in_image.p[1]<<" ,r=";

    //laserPoint 在　 map 矩阵表示
    KDL::Frame frame_laserPoint_in_map=(*agvInMapFrame)*(*frame_laserlink_in_baselink)*frame_laserPoint_in_laserlink;
//     qDebug()<<"frame_baselink_in_world"<<"x="<<frame_baselink_in_world.p[0]<<" ,y="<<frame_baselink_in_world.p[1]<<" ,r=";

    double tmp_roll,tmp_pitch,tmp_yaw;
    pointOut.pose_x=frame_laserPoint_in_map.p[0];
    pointOut.pose_y=frame_laserPoint_in_map.p[1];
    frame_laserPoint_in_map.M.GetRPY(tmp_roll,tmp_pitch,tmp_yaw);
    pointOut.pose_r=tmp_yaw;

    //for test
//    KDL::Frame frame_laserlink_in_map=(*agvInWorldFrame)*(*frame_laserlink_in_baselink);
//    frame_laserlink_in_map.M.GetRPY(tmp_roll,tmp_pitch,tmp_yaw);


    return 1;
}



int ReflectorMatch::calculateLaserFramePose(const QVector<PointMatchInfo> &matchInfo, KDL::Frame *frame_laser,
                                            int &validCrossPointCountOut)
{

//    QVector<VehiclePosition> tmpCrossPoints;
    QVector<CrossPointInfo> tmpCrossInfoList;

    VehiclePosition tmpPoint;
    CrossPointInfo tmpCrossInfo;
    struct circle_t circles[2];
    struct point_t points[2];
    for(int i=0;i<matchInfo.size()-1;i++)
    {
        if(1==debugFlag)
        {
            qDebug()<<"matchInfo.size"<<matchInfo.size();//一个参考点和多个激光点匹配导致圆心相同，半径不同。
        }

        for(int j=i+1;j<matchInfo.size();j++)
        {
            if(1==debugFlag)
            {
                qDebug()<<"i"<<i<<"j"<<j;
            }

            circles[0].r=matchInfo[i].laserPoint.laserRangeValue;
            circles[0].center.x=matchInfo[i].referencePoint.pose_x;
            circles[0].center.y=matchInfo[i].referencePoint.pose_y;

            circles[1].r=matchInfo[j].laserPoint.laserRangeValue;
            circles[1].center.x=matchInfo[j].referencePoint.pose_x;
            circles[1].center.y=matchInfo[j].referencePoint.pose_y;
            tmpCrossInfo.r1=circles[0].r;
            tmpCrossInfo.r2=circles[1].r;

            switch (insect(circles, points))
            {
                case -1:
                {
                    if(1==debugFlag)
                    {
                          qDebug()<<"THE CIRCLES ARE THE SAME/n";
                    }
                }

                    break;
                case 0:
                {
                    if(1==debugFlag)
                    {
                        qDebug()<<"0个交点, "<<"1圆心"<< circles[0].center.x<< circles[0].center.y<<circles[0].r<<";"
                                  <<"2圆心"<< circles[1].center.x<< circles[1].center.y<<circles[1].r;
                    }
                }

                    break;
                case 1:
                {
                    if(1==debugFlag)
                    {
                          qDebug()<<"1 个交点 "<< points[0].x<<points[0].y;
                    }
                }

                    break;
                case 2:
                    if(1==debugFlag)
                    {
                          qDebug()<<"2个交点, "<<"("<< points[0].x<<points[0].y<<") ("<< points[1].x<<points[1].y<<")"
                                 <<"circles[0].r"<<circles[0].r<<"circles[1].r"<<circles[1].r;
                    }
//                    //２交点的间距小于阈值，则舍弃。
//                    double tmpCrossDistance=sqrt((points[0].x-points[1].x)*(points[0].x-points[1].x)
//                                                    +(points[0].y-points[1].y)*(points[0].y-points[1].y));

//                    if(tmpCrossDistance<D_MIN_QUALIFIED_CROSS_DISTANCE)
//                    {
//                        if(1==debugFlag)
//                        {
//                              qDebug()<<"#交点ＮＧ,tmpCrossDistance<D_MIN_QUALIFIED_CROSS_DISTANCE tmpCrossDistance="
//                                     <<tmpCrossDistance<<D_MIN_QUALIFIED_CROSS_DISTANCE;
//                        }
//                        break;
//                    }



                    //交点与设定位置偏差判断。
                     double laserError_x[2],laserError_y[2];

                     //求距离
                     for(int k=0;k<2;k++)
                     {
                         laserError_x[k]=points[k].x-laserlinkInMapPosition.x;
                         laserError_y[k]=points[k].y-laserlinkInMapPosition.y;
                     }
                     //2个圆交点，只能取１个距离最接近的作为最优位置。
                     int tmpBestIndex;
                     if(fabs(laserError_x[0])+fabs(laserError_y[0])<fabs(laserError_x[1])+fabs(laserError_y[1]))
                     {
                         tmpBestIndex=0;
                     }
                     else
                     {
                         tmpBestIndex=1;
                     }

                     //交点夹角合格判断
                     double tmpAngleOut;
                     if(false==isCrossAngleOk(detectPara.minCorssAngle,circles,points[tmpBestIndex],tmpAngleOut))
                     {
                         if(1==debugFlag)
                         {
                               qDebug()<<"2交点夹角不合格 tmpAngleOut(degree)"<<180*tmpAngleOut/M_PI
                                      <<"minCorssAngle"<<180*detectPara.minCorssAngle/M_PI;
                         }
                         break;
                     }
                     tmpCrossInfo.angle=tmpAngleOut;
                     //判断计算位置和初始位置的位置误差，姿态误差
                     if(fabs(laserError_x[tmpBestIndex])<poseErrorInfo.x
                             && fabs(laserError_y[tmpBestIndex])<poseErrorInfo.y)
                     {
                         tmpPoint.x=points[tmpBestIndex].x;
                         tmpPoint.y=points[tmpBestIndex].y;
                         //姿态1求解
                         double tmpAngle1=atan2(circles[0].center.y-tmpPoint.y,circles[0].center.x-tmpPoint.x);
                           double tmpRotate1;
                          if(fabs(laser_roll)>0.1 || fabs(laser_pitch)>0.1)//倒置安装
                          {
                              tmpRotate1=tmpAngle1+matchInfo[i].laserPoint.laserAngleValue;
                          }
                          else
                          {
                              tmpRotate1=tmpAngle1-matchInfo[i].laserPoint.laserAngleValue;
                          }
                          //姿态2求解
                          double tmpAngle2=atan2(circles[1].center.y-tmpPoint.y,circles[1].center.x-tmpPoint.x);
                          double tmpRotate2;
                           if(fabs(laser_roll)>0.1 || fabs(laser_pitch)>0.1)//倒置安装
                           {
                               tmpRotate2=tmpAngle2+matchInfo[j].laserPoint.laserAngleValue;
                           }
                           else
                           {
                               tmpRotate2=tmpAngle2-matchInfo[j].laserPoint.laserAngleValue;
                           }


                          FilteringAlgorithm tmpfiltering;
                          QVector<double> dataIn;
                          dataIn.append(tmpRotate1);
                          dataIn.append(tmpRotate2);
                          tmpPoint.rotate=tmpfiltering.arithmeticMeanfilter_angle(dataIn);
                          double tmpAngleBias=angleRotationJudgement(tmpPoint.rotate,laserlinkInMapPosition.rotate);
                          if(fabs(tmpAngleBias)<poseErrorInfo.rotate)
                          {
//                              tmpCrossPoints.append(tmpPoint);
                              tmpCrossInfo.pose=tmpPoint;
                              tmpCrossInfoList.append(tmpCrossInfo);
                              if(1==debugFlag)
                              {
                                    qDebug()<<"交点合格: 位置偏差laserError_x"<<laserError_x[tmpBestIndex]
                                              <<"laserError_y"<<laserError_y[tmpBestIndex]
                                            <<"tmpAngle1"<<tmpAngle1<<"rotate1"<<tmpRotate1
                                              <<"tmpAngle2"<<tmpAngle2<<"rotate2"<<tmpRotate2
                                             <<"角度偏差tmpAngleBias"<<180.0*tmpAngleBias/M_PI
                                               <<"交点夹角"<<180*tmpAngleOut/M_PI<<"度";
                              }
                          }
                          else
                          {
                              if(1==debugFlag)
                              {
                                    qDebug()<<"交点角度偏差不合格 tmpAngleBias"<<180.0*tmpAngleBias/M_PI<<"度";
                              }
                          }


                     }
                     else
                     {
                         if(1==debugFlag)
                         {
                               qDebug()<<"交点距离偏差不合格,laserError_x "<<laserError_x[tmpBestIndex]
                                       <<"laserError_y"<<laserError_y[tmpBestIndex];
                         }
                     }

                break;
            }
        }
    }

    checkCrossPoints(tmpCrossInfoList,detectPara.maxmumReferencePointCount , detectPara.idealRadius,
                     detectPara.idealMatchNum);

    validCrossPointCountOut=tmpCrossInfoList.size();

    if(tmpCrossInfoList.size()>0)
    {
        VehiclePosition tmpPoint;
        crossPointFiltering(tmpCrossInfoList,tmpPoint);
        frame_laser->p[0]=tmpPoint.x;
        frame_laser->p[1]=tmpPoint.y;
        frame_laser->p[2]=0;
        KDL::Rotation tmpRotation;
        if(fabs(laser_roll)>0.1 || fabs(laser_pitch)>0.1)//倒置安装
        {
            tmpRotation=tmpRotation.RPY(-M_PI,0,tmpPoint.rotate);
        }
        else
        {
            tmpRotation=tmpRotation.RPY(0,0,tmpPoint.rotate);
        }

        frame_laser->M=tmpRotation;//
        if(1==debugFlag)
        {
            qDebug()<<"sucess,交点平均后的位置"<<frame_laser->p[0]<<frame_laser->p[1]
                   <<tmpPoint.rotate <<"　，有效交点size"<<tmpCrossInfoList.size();
        }
    }
    else
    {
        if(1==debugFlag)
        {
            qDebug()<<"failed，有效圆交点为0";
        }
        return 0;
    }


    return 1;
}

int ReflectorMatch::comparePoints(const QVector<ReflectMarkInfo> &referenceMarksIn, const QVector<VehiclePosition> &laserMarksIn,
                                  KDL::Frame *frame_laser_to_referenceMark)
{
    //穷举出参考三角形边长
    QVector<CompareLengthInfo> referenceLengths, laserMarkLengths;

    CompareLengthInfo tmpLengthInfo;
    for(int i=0;i<referenceMarksIn.size()-1;i++)
    {
        for(int j=i+1;j<referenceMarksIn.size();j++)
        {
            tmpLengthInfo.distance=(referenceMarksIn[i].pose_x-referenceMarksIn[j].pose_x)*(referenceMarksIn[i].pose_x-referenceMarksIn[j].pose_x)
                    +(referenceMarksIn[i].pose_y-referenceMarksIn[j].pose_y)*(referenceMarksIn[i].pose_y-referenceMarksIn[j].pose_y);
            tmpLengthInfo.source1Index=i;
            tmpLengthInfo.source2Index=j;
            referenceLengths.append(tmpLengthInfo);
            
        }
    }
    
    //穷举出反射三角形边长
    for(int i=0;i<laserMarksIn.size()-1;i++)
    {
        for(int j=i+1;j<laserMarksIn.size();j++)
        {
            tmpLengthInfo.distance=(laserMarksIn[i].x-laserMarksIn[j].x)*(laserMarksIn[i].x-laserMarksIn[j].x)
                    +(laserMarksIn[i].y-laserMarksIn[j].y)*(laserMarksIn[i].y-laserMarksIn[j].y);
            tmpLengthInfo.source1Index=i;
            tmpLengthInfo.source2Index=j;
            laserMarkLengths.append(tmpLengthInfo);
            
        }
    }
    
    //反射三角边长和所有参考边长比较
    QVector<QVector<LengthErrorInfo> > lengErrors;
    LengthErrorInfo tmpError;
    for(int i=0;i<laserMarkLengths.size();i++)
    {
        QVector<LengthErrorInfo> tmpLengthInfo;
        for(int j=0;j<referenceLengths.size();j++)
        {

            tmpError.distanceError=fabs(laserMarkLengths[i].distance-referenceLengths[j].distance);
            //不符合误差要求的剔除
            if(tmpError.distanceError<detectPara.maxMatchLineDistanceOffset)
            {
                tmpError.line1Index=i;
                tmpError.line1StartIndex=laserMarkLengths[i].source1Index;
                tmpError.line1EndIndex=laserMarkLengths[i].source2Index;
                tmpError.line2Index=j;
                tmpError.line2StartIndex=laserMarkLengths[j].source1Index;
                tmpError.line2EndIndex=laserMarkLengths[j].source2Index;
                tmpLengthInfo.append(tmpError);
            }
            
            
        }
        lengErrors.append(tmpLengthInfo);
    }
    
    //找出符合长度误差的点
    QVector<PointMapInfo> mapInfo;
    calculatePointMap(lengErrors,mapInfo);

    //todo
    
    
}

int ReflectorMatch::calculatePointMap(QVector<QVector<LengthErrorInfo> > &lengErrorsIn,QVector<PointMapInfo> &mapInfoOut)
{
    
}

double ReflectorMatch::angleRotationJudgement(double targetAngle, double initialAngle)
{
    double angle1,angle2,angle3;
    angle1=targetAngle-initialAngle;
    angle2=angle1+2*M_PI;
    angle3=angle1-2*M_PI;
    double minNum;
    if(fabs(angle1)<=fabs(angle2))
    {
        minNum=angle1;
    }
    else
    {
        minNum=angle2;
    }

    if(fabs(angle3)<=fabs(minNum))
    {
        minNum=angle3;
    }

    return minNum;

}

int ReflectorMatch::crossPointFiltering(const QVector<CrossPointInfo> &crossPoints, VehiclePosition &pointOut)
{
    //考虑是否要去掉角度偏差较大的值。
    VehiclePosition sumPoint;//角度不能直接平均,need todo
    for(int i=0;i<crossPoints.size();i++)
    {
        sumPoint.x+=crossPoints[i].pose.x;
        sumPoint.y+=crossPoints[i].pose.y;
    }
    if(crossPoints.size()>0)
    {
        pointOut.x=sumPoint.x/crossPoints.size();
        pointOut.y=sumPoint.y/crossPoints.size();
//        pointOut.rotate=crossPoints[0].rotate;
        FilteringAlgorithm tmpFilter;
        QVector<double> dataIn;
        for(int i=0;i<crossPoints.size();i++)
        {
            dataIn.append(crossPoints[i].pose.rotate);
        }
        pointOut.rotate=tmpFilter.arithmeticMeanfilter_angle(dataIn);
    }
    return 1;
}


int ReflectorMatch::calculateAgvPosition_byReflect(KDL::Frame *frame_laselink_in_map,
                                                   VehiclePosition &baselinkPoseOut)
{
    KDL::Frame tmpFrame=(*frame_laselink_in_map)*(*frame_baselink_in_laserlink);

    //check x,y,rotate的变换量，抗是否符合要求
    double tmp_roll,tmp_pitch,tmp_yaw;
    tmpFrame.M.GetRPY(tmp_roll,tmp_pitch,tmp_yaw);
    double xTolorence,yTolorence,rTolorence;
    xTolorence=fabs(tmpFrame.p[0]-baselinkPoseOut.x);
    yTolorence=fabs(tmpFrame.p[1]-baselinkPoseOut.y);
    rTolorence=fabs(angleRotationJudgement(baselinkPoseOut.rotate,tmp_yaw));
    if( xTolorence<poseErrorInfo.x
            && yTolorence<poseErrorInfo.y
            && rTolorence<poseErrorInfo.rotate)
    {
        baselinkPoseOut.rotate=tmp_yaw;
        baselinkPoseOut.x=tmpFrame.p[0];
        baselinkPoseOut.y=tmpFrame.p[1];
        return 1;
    }
    if(1==debugFlag)
    {
           qDebug()<<"failed, 平均后的交点误差超过允许值,当前误差　x"<<xTolorence<<" y"<<yTolorence<<" r"<<rTolorence;
    }

    return -1;


}

int ReflectorMatch::calculateAgvPosition_byOdom(PositionInfo &vehiclePositionInfoOut)
{
      //            agvInMapFrame=frame_odom_in_map*frame_baselink_in_odom;
    KDL::Frame agvInMapFrame=(*frame_odom_in_map)*(*frame_baselink_in_odom);
//    vehiclePositionInfoOut.isUpdateSucess=false;
    vehiclePositionInfoOut.maxMatchError=fabs(poseErrorInfo.x)+fabs(poseErrorInfo.y)+fabs(poseErrorInfo.rotate);
    //vehiclePositionInfoOut.matchScore=10/vehiclePositionInfoOut.maxMatchError;
    vehiclePositionInfoOut.position.x=agvInMapFrame.p[0];
    vehiclePositionInfoOut.position.y=agvInMapFrame.p[1];

    double tmp_roll,tmp_pitch,tmp_yaw;
    agvInMapFrame.M.GetRPY(tmp_roll,tmp_pitch,tmp_yaw);
    vehiclePositionInfoOut.position.rotate=tmp_yaw;

    //求激光雷达位置
    calculateLaserLinkPosition(vehiclePositionInfoOut.position,laserlinkInMapPosition);

    //计算位置估计误差
    calculateAgvPositionProbolityError(agvInMapPosition_old.position,agvInMapPosition.position,
                                       poseErrorInfo);
    calculateAgvPositionProbolityError(agvInMapPosition_old.position,agvInMapPosition.position,
                                       matchDebugInfo.odomAccumulatedError);
    agvInMapPosition_old=agvInMapPosition;

    return 1;

}

int ReflectorMatch::calculateCorrectPosition(PositionInfo amclPositionIn,
                                             PositionInfo &reflectPositionInOut)
{

    if(amclPositionIn.isUpdateSucess && amclPositionIn.matchScore>detectPara.qualifyAmclMinCertaintyScore)
    {
        if(matchDebugInfo.odomAccumulatedError.x>detectPara.correctOdomErrorDistanceThreshold
                || matchDebugInfo.odomAccumulatedError.y>detectPara.correctOdomErrorDistanceThreshold
                || matchDebugInfo.odomAccumulatedError.rotate>detectPara.correctOdomErrorAngleThreshold)
        {


            double tmpDistance=sqrt((amclPositionIn.position.x-reflectPositionInOut.position.x)*
                                    (amclPositionIn.position.x-reflectPositionInOut.position.x)+
                                    (amclPositionIn.position.y-reflectPositionInOut.position.y)*
                                    (amclPositionIn.position.y-reflectPositionInOut.position.y));
            double tmpAngle=angleRotationJudgement(amclPositionIn.position.rotate,
                                                   reflectPositionInOut.position.rotate);
            if(tmpDistance<detectPara.correctAllowMaxDistanceOffset
                    &&fabs(tmpAngle)<detectPara.correctAllowMaxAngleOffset)
            {
                matchDebugInfo.correctPositionOffset.x=amclPositionIn.position.x-reflectPositionInOut.position.x;
                matchDebugInfo.correctPositionOffset.y=amclPositionIn.position.y-reflectPositionInOut.position.y;
                matchDebugInfo.correctPositionOffset.rotate=tmpAngle;
                matchDebugInfo.correctCount++;
                matchDebugInfo.isCorrectSucess=1;

                //做平滑过渡. 已经有了里程计平滑，最终位置平滑．
                reflectPositionInOut.position.x+=matchDebugInfo.correctPositionOffset.x/detectPara.correctFilterCount;
                reflectPositionInOut.position.y+=matchDebugInfo.correctPositionOffset.y/detectPara.correctFilterCount;
                reflectPositionInOut.position.rotate+=tmpAngle/detectPara.correctFilterCount;

                return 1;
            }
            else
            {
                matchDebugInfo.isCorrectSucess=-1;
                qDebug()<<"ReflectorMatch::calculateCorrectPosition:: bias to big,tmpDistance"<<tmpDistance<<"tmpAngle"<<tmpAngle;
                return -1;
            }
        }


    }
    matchDebugInfo.isCorrectSucess=0;
    return 0;
}

int ReflectorMatch::calculateOffsetMatrix(VehiclePosition agvInMapPositionIn, KDL::Frame *frame_baselink_in_odomIn,
                                          KDL::Frame *frame_odom_in_mapOut)
{
    //agvInMapFrame
    KDL::Frame agvInMapFrame;
    agvInMapFrame.p.data[0]=agvInMapPositionIn.x;
    agvInMapFrame.p.data[1]=agvInMapPositionIn.y;
    agvInMapFrame.p.data[2]=0;
    agvInMapFrame.M=agvInMapFrame.M.RPY(0,0,agvInMapPositionIn.rotate);

    //            agvInMapFrame=frame_odom_in_map*frame_baselink_in_odom;
    KDL::Frame tmpOffsetFrame;
    tmpOffsetFrame=agvInMapFrame*frame_baselink_in_odomIn->Inverse();

    //对校准偏差做滤波处理．sick和r2000,静止时的位置变换范围都较大．
    double tmpOffsetX,tmpOffsetY,tmpOffsetR;
    tmpOffsetX=tmpOffsetFrame.p[0];
    tmpOffsetY=tmpOffsetFrame.p[1];
    double tmp_roll,tmp_pitch,tmp_yaw;
    tmpOffsetFrame.M.GetRPY(tmp_roll,tmp_pitch,tmp_yaw);
    tmpOffsetR=tmp_yaw;
    if(initialPositionCount>0)
    {
        filtering_position_x->resetAll();
        filtering_position_y->resetAll();
        filtering_position_r->resetAll();
        initialPositionCount--;
    }


    //medianMean//arithmeticMeanfilter,原地旋转时，如果更新频率太快，滤波长度过短３，可能导致控制失去稳定性．
    odomOffset_x=filtering_position_x->medianMean(tmpOffsetX,detectPara.odomOffsetFilterCount);
    odomOffset_y=filtering_position_y->medianMean(tmpOffsetY,detectPara.odomOffsetFilterCount);
    odomOffset_r=filtering_position_r->medianMean_angle(tmpOffsetR,detectPara.odomOffsetFilterCount);


//    qDebug()<<"odomOffset_x"<<odomOffset_x<<tmpOffsetX<<"odomOffset_y"<<odomOffset_y <<tmpOffsetY
//           <<"odomOffset_r"<<odomOffset_r<<tmpOffsetR<<detectPara.odomOffsetFilterCount;


    (*frame_odom_in_mapOut).p.data[0]=odomOffset_x;
    (*frame_odom_in_mapOut).p.data[1]=odomOffset_y;
    (*frame_odom_in_mapOut).p.data[2]=0;
    (*frame_odom_in_mapOut).M=(*frame_odom_in_mapOut).M.RPY(0,0,odomOffset_r);


    return 1;

}

int ReflectorMatch::readConfigFile(QString filePathIn,QString &readComent)
{
    DomParser xmlParser;

    bool ok = xmlParser.openXml(filePathIn, QIODevice::ReadOnly );
    if( !ok )//无法打开文件！
    {
       qDebug()<<"error, ReflectorMatch::readConfigFile open file error！";
       return -1;
    }

    //CalculateNode
    QDomNode  inNode;
    inNode=xmlParser.findSubNode(xmlParser.getRootDomElement(), "CalculateNode", ok);
    if( !ok )
    {
        qDebug()<<"error, CalculateNode node error！";
        return -1;
    }
    xmlParser.readXml( xmlParser.findSubNode(inNode, "allowMinMarkDistance", ok), detectPara.allowMinMarkDistance );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }
    xmlParser.readXml( xmlParser.findSubNode(inNode, "minimumReferencePointCount", ok),detectPara. minimumReferencePointCount );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "maxmumReferencePointCount", ok), detectPara.maxmumReferencePointCount );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "idealRadius", ok), detectPara.idealRadius );
    if( !ok )
    {
        qDebug()<<"error,  node idealRadius error！";
        readComent+=" idealRadius";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "idealMatchNum", ok), detectPara.idealMatchNum );
    if( !ok )
    {
        qDebug()<<"error,  node  idealMatchNum error！";
        readComent+=" idealMatchNum";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "maxMatchLineDistanceOffset", ok), detectPara.maxMatchLineDistanceOffset );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "initial_AgvPositionOffset", ok), detectPara.initial_AgvPositionOffset );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "mached_AgvPositionOffset", ok), detectPara.mached_AgvPositionOffset );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "mached_AgvAngleOffset", ok), detectPara.mached_AgvAngleOffset );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "odomErrorRatio_position", ok), detectPara.odomErrorRatio_position );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "odomErrorRatio_angle", ok), detectPara.odomErrorRatio_angle );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "initial_AgvAngleOffset", ok), detectPara.initial_AgvAngleOffset );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "laserIntensityLoseRatio", ok), detectPara.laserIntensityLoseRatio );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "matchColumnMarkDiameterTolorenceRatio", ok), detectPara.matchColumnMarkDiameterTolorenceRatio );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "matchPlaneMarkMinWidth", ok), detectPara.matchPlaneMarkMinWidth );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "matchPlaneMarkMaxWidth", ok), detectPara.matchPlaneMarkMaxWidth );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "odomOffsetFilterCount", ok), detectPara.odomOffsetFilterCount );
    if( !ok )
    {
        qDebug()<<"error, odomOffsetFilterCount node error！";
        detectPara.odomOffsetFilterCount=30;
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "minCorssAngle", ok), detectPara.minCorssAngle );
    if( !ok )
    {
        qDebug()<<"error, minCorssAngle node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "isCorrectByAmcl", ok), detectPara.isCorrectByAmcl );
    if( !ok )
    {
        qDebug()<<"error, isCorrectByAmcl node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "qualifyAmclMinCertaintyScore", ok), detectPara.qualifyAmclMinCertaintyScore );
    if( !ok )
    {
        qDebug()<<"error, qualifyAmclMinCertaintyScore node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "correctFilterCount", ok), detectPara.correctFilterCount );
    if( !ok )
    {
        qDebug()<<"error, correctFilterCount node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "correctAllowMaxDistanceOffset", ok), detectPara.correctAllowMaxDistanceOffset );
    if( !ok )
    {
        qDebug()<<"error, correctAllowMaxDistanceOffset node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "correctAllowMaxAngleOffset", ok),detectPara.correctAllowMaxAngleOffset );
    if( !ok )
    {
        qDebug()<<"error, correctAllowMaxAngleOffset node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "correctOdomErrorDistanceThreshold", ok),detectPara.correctOdomErrorDistanceThreshold );
    if( !ok )
    {
        qDebug()<<"error, correctOdomErrorDistanceThreshold node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "correctOdomErrorAngleThreshold", ok),detectPara.correctOdomErrorAngleThreshold );
    if( !ok )
    {
        qDebug()<<"error, correctOdomErrorAngleThreshold node error！";
        return -1;
    }

    //PlaneMark
    inNode;
    inNode=xmlParser.findSubNode(xmlParser.getRootDomElement(), "PlaneMark", ok);
    if( !ok )
    {
        qDebug()<<"error, PlaneMark node error！";
        return -1;
    }
    QDomNodeList tmpNodelist=inNode.childNodes();

    planeMarkWidth.clear();
    int tmpId;
    double tmpWidth;
    for(int i=0;i<tmpNodelist.size();i++)
    {
        //节点ｒｅａｄ数据 typeId
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "typeId", ok), tmpId );
        if( !ok )
        {
            qDebug()<<"error, typeId node error！";
            return -1;
        }
        //节点ｒｅａｄ数据 width
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "width", ok), tmpWidth );
        if( !ok )
        {
            qDebug()<<"error, width node error！";
            return -2;
        }
        planeMarkWidth.insert(tmpId,tmpWidth);
    }

    //ValidPointCheck
    inNode;
    inNode=xmlParser.findSubNode(xmlParser.getRootDomElement(), "ValidPointCheck", ok);
    if( !ok )
    {
        qDebug()<<"error, ValidPointCheck node error！";
        return -1;
    }
    tmpNodelist=inNode.childNodes();

    validPointCheckList.clear();
    ValidPointCheck tmpCheck;
    for(int i=0;i<tmpNodelist.size();i++)
    {
        //节点ｒｅａｄ数据 typeId
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "distance", ok), tmpCheck.distance );
        if( !ok )
        {
            qDebug()<<"error, distance node error！";
            return -1;
        }
        //节点ｒｅａｄ数据 width
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "minCount", ok), tmpCheck.minCount );
        if( !ok )
        {
            qDebug()<<"error, minCount node error！";
            return -2;
        }
        validPointCheckList.append(tmpCheck);
    }

    //ColumnMark
    inNode=xmlParser.findSubNode(xmlParser.getRootDomElement(), "ColumnMark", ok);
    if( !ok )
    {
        qDebug()<<"error, ColumnMark node error！";
        return -3;
    }
    tmpNodelist=inNode.childNodes();

    columnMarkDiameter.clear();
    double tmpDiameter;
    for(int i=0;i<tmpNodelist.size();i++)
    {
        //节点ｒｅａｄ数据 typeId
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "typeId", ok), tmpId );
        if( !ok )
        {
            qDebug()<<"error, typeId node error！";
            return -1;
        }
        //节点ｒｅａｄ数据 width
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "diameter", ok), tmpDiameter );
        if( !ok )
        {
            qDebug()<<"error, diameter node error！";
            return -1;
        }
        columnMarkDiameter.insert(tmpId,tmpDiameter);
    }

    inNode=xmlParser.findSubNode(xmlParser.getRootDomElement(), "LaserFilter", ok);
    if( !ok )
    {
        qDebug()<<"error, LaserFilter node error！";
        return -1;
    }
    tmpNodelist=inNode.childNodes();
    int filterType;
    LaserFilterParameter tmp_filterParmeter;
    laserFilter_Parameter.clear();
    for(int i=0;i<tmpNodelist.size();i++)
    {
        //节点ｒｅａｄ数据 filterType
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "filterType", ok), filterType );
        if( !ok )
        {
            qDebug()<<"error,  node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "minumIntensity", ok), tmp_filterParmeter.minumIntensity );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "maxLaserRange", ok), tmp_filterParmeter.maxLaserRange );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "minLaserRange", ok), tmp_filterParmeter.minLaserRange );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "minAngle", ok), tmp_filterParmeter.minAngle );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "maxAngle", ok), tmp_filterParmeter.maxAngle );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "region_front", ok), tmp_filterParmeter.region_front );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "region_back", ok), tmp_filterParmeter.region_back );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "region_left", ok), tmp_filterParmeter.region_left );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "region_right", ok), tmp_filterParmeter.region_right );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }

        laserFilter_Parameter.insert(filterType,tmp_filterParmeter);
    }


    xmlParser.closeXml( );

    return 1;

}

int ReflectorMatch::writeLaserFilterConfigFile(QString filePathIn,
                                               QMap<int, LaserFilterParameter> laserFilter_ParameterIn)
{
    DomParser xmlParser;

    bool ok = xmlParser.openXml(filePathIn, QIODevice::ReadWrite );
    if( !ok )//无法打开文件！
    {
       qDebug()<<"error, ReflectorMatch::writeLaserFilterConfigFile open file error！";
       return -1;
    }

    QDomNode  inNode;
    inNode=xmlParser.findSubNode(xmlParser.getRootDomElement(), "LaserFilter", ok);
    if( !ok )
    {
        qDebug()<<"error, LaserFilter node error！";
        return -1;
    }
    QDomNodeList tmpNodelist=inNode.childNodes();
    int filterType;
    LaserFilterParameter tmp_filterParmeter;
    for(int i=0;i<tmpNodelist.size();i++)
    {
        //节点ｒｅａｄ数据 filterType
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "filterType", ok), filterType );
        if( !ok )
        {
            qDebug()<<"error,  node error！";
            return -1;
        }
        tmp_filterParmeter=laserFilter_ParameterIn.value(filterType);
        xmlParser.writeXml( xmlParser.findSubNode(tmpNodelist.at(i), "minumIntensity", ok), tmp_filterParmeter.minumIntensity );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.writeXml( xmlParser.findSubNode(tmpNodelist.at(i), "maxLaserRange", ok), tmp_filterParmeter.maxLaserRange );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.writeXml( xmlParser.findSubNode(tmpNodelist.at(i), "minLaserRange", ok), tmp_filterParmeter.minLaserRange );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.writeXml( xmlParser.findSubNode(tmpNodelist.at(i), "minAngle", ok), tmp_filterParmeter.minAngle );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.writeXml( xmlParser.findSubNode(tmpNodelist.at(i), "maxAngle", ok), tmp_filterParmeter.maxAngle );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.writeXml( xmlParser.findSubNode(tmpNodelist.at(i), "region_front", ok), tmp_filterParmeter.region_front );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.writeXml( xmlParser.findSubNode(tmpNodelist.at(i), "region_back", ok), tmp_filterParmeter.region_back );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.writeXml( xmlParser.findSubNode(tmpNodelist.at(i), "region_left", ok), tmp_filterParmeter.region_left );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.writeXml( xmlParser.findSubNode(tmpNodelist.at(i), "region_right", ok), tmp_filterParmeter.region_right );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
    }


    xmlParser.closeXml(true);

    return 1;
}

int ReflectorMatch::writeCommonCalculateConfigFile(QString filePathIn, ReflectCommonDetectPara detectParaIn)
{
    DomParser xmlParser;

    bool ok = xmlParser.openXml(filePathIn, QIODevice::ReadWrite );
    if( !ok )//无法打开文件！
    {
       qDebug()<<"error, ReflectorMatch::writeCommonCalculateConfigFile open file error！";
       return -1;
    }

    //CalculateNode
    QDomNode  inNode;
    inNode=xmlParser.findSubNode(xmlParser.getRootDomElement(), "CalculateNode", ok);
    if( !ok )
    {
        qDebug()<<"error, CalculateNode node error！";
        return -1;
    }
    xmlParser.writeXml( xmlParser.findSubNode(inNode, "allowMinMarkDistance", ok), detectParaIn.allowMinMarkDistance );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }
    xmlParser.writeXml( xmlParser.findSubNode(inNode, "minimumReferencePointCount", ok),detectParaIn. minimumReferencePointCount );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "idealRadius", ok), detectParaIn.idealRadius );
    if( !ok )
    {
        qDebug()<<"error,  node idealRadius error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "idealMatchNum", ok), detectParaIn.idealMatchNum );
    if( !ok )
    {
        qDebug()<<"error,  node idealMatchNum error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "maxmumReferencePointCount", ok), detectParaIn.maxmumReferencePointCount );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "maxMatchLineDistanceOffset", ok), detectParaIn.maxMatchLineDistanceOffset );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "initial_AgvPositionOffset", ok), detectParaIn.initial_AgvPositionOffset );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "mached_AgvPositionOffset", ok), detectParaIn.mached_AgvPositionOffset );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "mached_AgvAngleOffset", ok), detectParaIn.mached_AgvAngleOffset );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "odomErrorRatio_position", ok), detectParaIn.odomErrorRatio_position );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "odomErrorRatio_angle", ok), detectParaIn.odomErrorRatio_angle );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "initial_AgvAngleOffset", ok), detectParaIn.initial_AgvAngleOffset );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "laserIntensityLoseRatio", ok), detectParaIn.laserIntensityLoseRatio );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "matchColumnMarkDiameterTolorenceRatio", ok), detectParaIn.matchColumnMarkDiameterTolorenceRatio );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "matchPlaneMarkMinWidth", ok), detectParaIn.matchPlaneMarkMinWidth );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "matchPlaneMarkMaxWidth", ok), detectParaIn.matchPlaneMarkMaxWidth );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "odomOffsetFilterCount", ok), detectParaIn.odomOffsetFilterCount );
    if( !ok )
    {
        qDebug()<<"error, odomOffsetFilterCount node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "minCorssAngle", ok), detectParaIn.minCorssAngle );
    if( !ok )
    {
        qDebug()<<"error, minCorssAngle node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "isCorrectByAmcl", ok), detectParaIn.isCorrectByAmcl );
    if( !ok )
    {
        qDebug()<<"error, isCorrectByAmcl node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "qualifyAmclMinCertaintyScore", ok), detectParaIn.qualifyAmclMinCertaintyScore );
    if( !ok )
    {
        qDebug()<<"error, qualifyAmclMinCertaintyScore node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "correctFilterCount", ok), detectParaIn.correctFilterCount );
    if( !ok )
    {
        qDebug()<<"error, correctFilterCount node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "correctAllowMaxDistanceOffset", ok), detectParaIn.correctAllowMaxDistanceOffset );
    if( !ok )
    {
        qDebug()<<"error, correctAllowMaxDistanceOffset node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "correctAllowMaxAngleOffset", ok),detectParaIn.correctAllowMaxAngleOffset );
    if( !ok )
    {
        qDebug()<<"error, correctAllowMaxAngleOffset node error！";
        return -1;
    }


    xmlParser.writeXml( xmlParser.findSubNode(inNode, "correctOdomErrorDistanceThreshold", ok),detectParaIn.correctOdomErrorDistanceThreshold );
    if( !ok )
    {
        qDebug()<<"error, correctOdomErrorDistanceThreshold node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "correctOdomErrorAngleThreshold", ok),detectParaIn.correctOdomErrorAngleThreshold );
    if( !ok )
    {
        qDebug()<<"error, correctOdomErrorAngleThreshold node error！";
        return -1;
    }


    xmlParser.closeXml(true);

    return 1;
}

void ReflectorMatch::addMsg(int messageLevel, string componentName, string messageType, int messageCode, int robotIdIn,
                            double parameter1, double parameter2, int parameter3, int parameter4,QString comment)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageCode = messageCode;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotIdIn;

    string infomationStr;
    QString tmpStr;

    switch(messageCode)
    {
    case 22206:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",地图文件关联的反光标记文件不存在　！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22205:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",反光条模块，里程角度误差超过设定值(%1)　！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22204:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",加载地图反光柱标记数据失败，反光标记类别typeId(%1)不一致　！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22203:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",加载地图反光标记数据错误，同时存在平面反光标记和圆柱反光标记　！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22202:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",加载地图反光柱标记数据失败，不识别的标记类别　！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }

    case 22201:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",加载地图反光条标记数据失败，不识别的标记类别　！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }

    case 22200:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",反光条算法模块:reflectMatch.xml读取失败　！")+comment;
        infomationStr = tmpStr.toStdString();
        break;
    }




        default:
        {
            qDebug()<<"Coordinate::addMsg====not match error code";
            break;
        }
    }

    tmpMsg.MessageInformation = infomationStr;

    #ifndef MOTION_SERVER_UNIT_TEST
    MessageLog::getInstance()->addMessage(tmpMsg);
    #endif
}
